﻿using ACS.SPiiPlusNET;
using System;
using System.Collections.Generic;
using System.Threading;

namespace NDK.Motion.Card
{
    public enum ACS_MOTION_STATUS_ENUM
    {
        //Motion status bit number define.
        //MST is an integer array, with one element for each axis in the system.
        //The elements of which contain a set of bits that display the current motor state for the given axis are:
        MTS_ENABLED = 0,  //使能.0: motor is disabled;1: motor is enabled.
        MTS_OPEN = 1,   //  0: motor is operating with closed loop control;1: motor is operating with open loop control.
        MTS_INP = 4,  //In Pos;0: Motor is moving or is out of range;1: Motor is not moving and has reached the target position (see variables TARGRAD and SETTLE
        MTS_MOVE = 4,  //In Move;0: = Axis is not involved in a motion;1 = Axis is involved in a motion
        MTS_ACC = 6,  //In ACC;0 = Motor is not accelerating;1 = Motor is accelerating.
        //MTS_PFGMODE = 10,  //
    }
    public enum ACS_AST_STATUS_ENUM
    {
        //axis status bit number define.
        //AST is an integer array, with one element for each axis in the system, the elements of which contain a set of bits used for displaying the current axis state.
        AST_LEAD = 0,  //1 = axis is leading in a group
        AST_PEG = 2,   //1 = PEG is in progress
        AST_DC = 3,    //1 = Axis data collection is in progress
        AST_PEGREADY = 4,  //1 = all values are loaded and the Incremental/Random PEG is ready to respond to movement
        AST_MOVE = 5,  //1 = Axis is involved in a motion
        AST_ACC = 6,   //1 = Axis in accelerating motion state
        AST_BUILDUP = 7,//1 = Segments build-up
        AST_VELLOCK = 8,//1 = Slave is synchronized to master in velocity lock mode - slave velocity strictly follows the master velocity.
        AST_POSLOCK = 9,//1 = Slave is synchronized to master in position lock mode - slave position strictly follows the master position.
        AST_TRIGGER = 11,//1 = Produces an interrupt to the host application, enabled by IENA.26
        //The controller sets the bit to inform that a new segment is required to be provided by the application.
        //The bit is set starvation_margin ms before the starvation condition occurs. The starvation condition is indicated by #STARV bit. 
        AST_NEWSEGM = 16,
        //The controller sets the bit to indicate starvation condition. The starvation condition means that there are not enough further segments to continue the motion with required velocity.
        //In this case, the controller starts decelerating the motion with ½ JERK in order to prevent motion discontinuity and avoid mechanical jerks.
        //Once the application begins supplying segments at a sufficient rate, the controller returns the motion back to normal condition.
        //Note, that often the starvation condition causes inefficient velocity generation and increases the time required for completing the required motion path.
        AST_STARV = 17,
        AST_ENCWARN = 18,//Indicates if there is an encoder warning. Cleared by the ACSPL+ FCLEAR command.
        AST_ENC2WARN = 19,//Indicates if there is an secondary encoder warning. Cleared by the ACSPL+ FCLEAR command.
        AST_INRANGE = 20,//Laser In Range
        AST_LCTICKLE = 21,//0: tickle mode is off;1: tickle mode is active
        AST_LCMODUL = 22,//0: modulation is off;1: modulation is active
        AST_FOLLOWED = 23,//0: Axis in regular mode;1: Axis in slave mode and follows the profile generated by RTC6
        AST_HOLD = 24,//0: hold is off;1: hold is in progress
        AST_INHOMING = 25,//0: homing is not in process;1: homing is in process
        //0: dynamic error compensation is switched off
        //1: dynamic error compensation is switched on
        //The bit is set to 0 in the following cases:
        //	> 	Calling ERRORMAPOFF function
        //	> 	Calling ERRORUNMAP function (in case there are no other active dynamic error compensation zones of the specified axis)
        //	> 	During homing process initiated by HOME command (switched ON back at the end of the process
        //	> 	Encoder Error / Encoder Not Connected faults
        //	> 	Changing one of the encoder-related parameters (E_TYPE, E_FREQ, E_SCMUL, SLPBITS, E_AOFSS)
        //	> 	Changing encoder routing (SLPROUT)
        AST_DECOMPON = 26,

        AST_INSHAPE = 27,//0: Input Shaping not Active;1: Input Shaping Active
        AST_ENCPROC = 29//0: Encoder Initialization not in process;1: Encoder Initialization in process

    }
    public enum ACS_FAULT_STATUS_ENUM
    {
        //S_FAULT is a scalar integer variable consisting of a set of bits equating to the occurrence of faults.
        //S_FAULT has two categories of bits, Axis Faults and System Faults (faults that are not related to any specific axis).
        RL = 0,  //.Hardware Right Limit;1 = Right limit switch is activated.
        LL = 1,   //Hardware Left Limit；1 = Left limit switch is activated. 
        NT = 2,  //Network Error.1 = EtherCAT network error is activated.
        HOT = 4,  //Motor Overheat1 = Motor's temperature sensor indicates overheat.
        SRL = 5,  //Software Right Limit；1 = Axis reference position (RPOS) is greater than the software right limit margin (SRLIMIT).
        SLL = 6,//Software Left Limit；1 = Axis reference position (RPOS) is less than the software left limit margin (SLLIMIT).
        ENCNC = 7,//Encoder Not Connected；1 = Primary encoder (for digital encoder type only) is not connected.
        ENC2NC = 8,//Encoder 2 Not Connected；1 = Secondary encoder (for digital encoder type only) is not connected.
        DRIVE = 9,//Drive Fault；1 = Signal from the drive reports a failure.
        ENC = 10,//Encoder Error；1 = Primary encoder miscounts.
        ENC2 = 11,//Encoder 2 Error；1 = Secondary encoder miscounts.
        //Position Error
        //1 = Position error (PE) has occurred.
        //PE is defined by the following variables:
        //	> 	ERRI - Maximum position error while the axis is idle
        //	> 	ERRV - Maximum position error while the axis is moving with constant velocity
        //	> 	ERRA - Maximum position error while the axis is accelerating or decelerating
        //	> 	DELI - Delay on transition from ERRA to ERRI
        //	> 	DELV - Delay on transition from ERRA to ERRV
        PE = 12,
        //Critical Position Error
        //1 = Position error (#PE) exceeds the value of the critical limit.
        //# CPE errors occur outside normal range of operation and #CPE > #PE.
        //The critical limit depends on the axis state and is defined by the following variables:
        //> 	CERRI if the axis is idle (not moving)
        //> 	CERRV if the axis is moving with constant velocity
        //> 	CERRA if the axis is accelerating or decelerating
        //> 	DELI - Delay on transition from ERRA to CERRI
        //> 	DELV - Delay on transition from ERRA to CERRV
        CPE = 13,
        VL = 14,//Velocity Limit.1 = Absolute value of the reference velocity (RVEL) exceeds the limit defined by the XVEL parameter.
        AL = 15,//Acceleration Limit；1 = Absolute value of the reference acceleration (RACC) exceeds the limit defined by the XACC parameter.
        CL = 16,//Current Limit;1 = RMS current calculated in the Servo Processor exceeds the limit value defined by the XRMS parameter.
        SP = 17,//Servo Processor Alarm;1 = Axis Servo Processor loses its synchronization with the MPU. The fault indicates a fatal problem in the controller.
        HSSINC = 20,//HSSI Not Connected;1 = HSSI module is not connected.
        EXTNT = 23,//External Network Error
        TEMP = 24,//MPU Overheat Fault;        Activated at CPU temperature > 90°C or System temperature > 70°C° Default response - none.
        PROG = 25,//Program Fault;1 = Run time error occurs in one of the executing ACSPL+ programs.
        MEM = 26,//Memory Overflow;1 = User application requires too much memory. 
        TIME = 27,//MPU Overuse;1 = User application consumes too much time in the controller cycle. 
        ES = 28,//Hardware Emergency Stop;1 = ES signal is activated.
        INT = 29,//Servo Interrupt;1 = The servo interrupt that defines the controller cycle is not generated. The fault indicates a fatal controller problem.
        INTGR = 30,//File Integrity;1 = The integrity of the user application in controller RAM is checked by the controller at power-up and whenever an #IR Terminal command is issued. 
        //Component Failure
        //1 = An MC4U hardware component other than the drive, such as the Power Supply, I/O card, or encoder card, has failed.
        //When the bus voltage is not supplied to the MC4U, a component failure fault is reported. The fault is system wide and prevents all axes from operating unless the fault is masked or bus voltage is supplied to the power supply.
        //When a component failure is reported, the affected power supply is identified by its address. To determine the faulty unit, use the MMI System Viewer and Diagnostics
        FAILURE = 31,

    }
    [Serializable]
    public class ACSController : IMotionCard, IOCard
    {

        public int StartAxisID { get; set; }
        public int TotalAxis { get; set; }
        [NonSerialized]
        private bool _Initialized;
        public bool Initialized
        {
            get
            {
                return _Initialized;
            }
            set { _Initialized = value; }
        }
        public int InputCount { get; set; }
        public int OutputCount { get; set; }
        public string CardType { get; set; }
        public string CardName { get; set; }
        public int CardID { get; set; }
        [NonSerialized]
        private Api Ch = new Api();
        private string ipAddress = "10.0.0.100";

        public bool CardInit(ref List<ICard> motionCards, int mode = 0)
        {
            if (Ch == null)
            {
                Ch = new Api();
            }
            try
            {
                try
                {
                    Ch.CloseComm();
                }
                catch { }
                Ch.OpenCommEthernet(ipAddress, (int)EthernetCommOption.ACSC_SOCKET_STREAM_PORT); //UDP
                TotalAxis = (int)GetSysinfo(13);
                if (TotalAxis > 0)
                {
                    this.CardID = 999;
                    this.CardType = "ACSController";
                    this.TotalAxis = (short)TotalAxis;//变量 ServoNum 接收 轴数量;
                    this.InputCount = 32;
                    this.OutputCount = 32;
                    this.StartAxisID = 0;
                    this.Initialized = true;
                    motionCards.Add(this);
                }
            }
            catch (Exception ex)
            {
                throw new Exception("ACSController CardInit Exception:" + ex.StackTrace);
            }
            return true;
        }
        public bool CardInit(int mode = 0)
        {
            if (Ch == null)
            {
                Ch = new Api();
            }
            try
            {
                try
                {
                    Ch?.CloseComm();
                }
                catch { }
                //Ch.OpenCommEthernet(ipAddress, (int)EthernetCommOption.ACSC_SOCKET_STREAM_PORT); //UDP
                Ch.OpenCommSimulator();
                TotalAxis = (int)GetSysinfo(13);
                if (TotalAxis > 0)
                {
                    this.Initialized = true;
                }
            }
            catch (Exception ex)
            {
                throw new Exception("ACSController CardInit Exception:" + ex.StackTrace);
            }
            return true;
        }
        public double GetSysinfo(int key)
        {
            double value = Ch.SysInfo(key);
            return value;

        }
        public int ReadIntIndex(string variableName, int index)
        {
            var result = Ch.ReadVariable(variableName, ProgramBuffer.ACSC_NONE, index, index);
            return Convert.ToInt32(result);
        }
        public bool CheckMoveDone(short axisNum)
        {
            bool isMoving = IsMoving(axisNum);
            if (isMoving == false)
            {
                return true;
            }
            return false;
        }

        public bool Finalize()
        {
            try
            {
                Ch.CloseComm();
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController Finalize Exception:" + ex.StackTrace);

            }
            return true;
        }

        public bool GetALMStatus(short axisId)
        {
            bool mstAlm = false;
            try
            {
                int ret = ReadIntIndex("FAULT", axisId);
                mstAlm = (ret & (1 << (int)ACS_FAULT_STATUS_ENUM.DRIVE)) != 0// 驱动器报错
                        || (ret & (1 << (int)ACS_FAULT_STATUS_ENUM.ENC)) != 0// 编码器1报错
                        || (ret & (1 << (int)ACS_FAULT_STATUS_ENUM.ENC2)) != 0 // 编码器2报错
                        || (ret & (1 << (int)ACS_FAULT_STATUS_ENUM.ENCNC)) != 0//编码器1未连接
                        || (ret & (1 << (int)ACS_FAULT_STATUS_ENUM.ENC2NC)) != 0 //编码器2未连接

                    ;
                if (mstAlm)
                {
                    return true;
                }
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController GetALMStatus Exception:" + ex.StackTrace);
            }

            return false;
        }

        public bool GetASTPStatus(short axisId)
        {
            throw new NotImplementedException();
        }

        public bool GetCmdPosition(short axisId, ref double commandPosition)
        {
            throw new NotImplementedException();
        }

        public bool GetCSTPStatus(short axisId)
        {
            throw new NotImplementedException();
        }

        public bool GetEMGStatus(short axisId)
        {
            bool mstAlm = false;
            try
            {
                mstAlm = (ReadIntIndex("FAULT", axisId) & (1 << (int)ACS_FAULT_STATUS_ENUM.ES)) != 0;
                if (mstAlm)
                {
                    return true;
                }
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController GetEMGStatus Exception:" + ex.StackTrace);
            }

            return false;
        }

        public bool GetErrorPosition(short axisId, ref double errorPosition)
        {
            throw new NotImplementedException();
        }
        public double ReadDoubleIndex(string variableName, int index)
        {
            var result = Ch.ReadVariable(variableName, ProgramBuffer.ACSC_NONE, index, index);
            return Convert.ToDouble(result);
        }

        public bool GetFeedbackPosition(short axisId, ref double feedbackPosition)
        {
            feedbackPosition = 9999.0;
            try
            {
                feedbackPosition = ReadDoubleIndex("FPOS", axisId);
            }
            catch (Exception ex)
            {
                throw new Exception("ACSController GetFeedbackPosition Exception:" + ex.StackTrace);

            }
            return true;
        }

        public bool GetFeedbackVelocity(short axisId, ref double feedbackVelocity)
        {
            throw new NotImplementedException();
        }

        public bool GetHMVStatus(short axisId)
        {
            bool isHoming = false;
            try
            {
                isHoming = (ReadIntIndex("AST", axisId) & (1 << (int)ACS_AST_STATUS_ENUM.AST_INHOMING)) != 0;
                if (isHoming)
                {
                    return true;
                }
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController GetHMVStatus Exception:" + ex.StackTrace);
            }

            return false;
        }

        public bool GetINPStatus(short axisId)
        {
            bool isINP = false;
            try
            {
                isINP = (ReadIntIndex("AST", axisId) & (1 << (int)ACS_MOTION_STATUS_ENUM.MTS_INP)) != 0;
                if (isINP)
                {
                    return true;
                }
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController GetINPStatus Exception:" + ex.StackTrace);
            }

            return false;
        }

        public bool GetMELStatus(short axisId)
        {
            bool isMel = false;
            try
            {
                isMel = (ReadIntIndex("AST", axisId) & (1 << (int)ACS_FAULT_STATUS_ENUM.LL)) != 0;
                if (isMel)
                {
                    return true;
                }
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController GetMELStatus Exception:" + ex.StackTrace);
            }

            return false;
        }

        public bool GetMotionIoSatus(short axisId, int motionIoStatusBit)
        {
            bool status = false;
            try
            {
                status = (ReadIntIndex("AST", axisId) & (1 << motionIoStatusBit)) != 0;
                if (status)
                {
                    return true;
                }
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController GetMotionIoSatus Exception:" + ex.StackTrace);
            }

            return false;
        }

        public bool GetMotionStatus(short axisId, int motionStatusBit)
        {
            bool status = false;
            try
            {
                status = (ReadIntIndex("MST", axisId) & (1 << motionStatusBit)) != 0;
                if (status)
                {
                    return true;
                }
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController GetMotionStatus Exception:" + ex.StackTrace);
            }

            return false;
        }

        public bool GetNSTPStatus(short axisId)
        {
            return !IsMoving(axisId);
        }

        public bool GetORGStatus(short axisId)
        {
            return false;
        }

        public bool GetPELStatus(short axisId)
        {
            bool isPel = false;
            try
            {
                isPel = (ReadIntIndex("AST", axisId) & (1 << (int)ACS_FAULT_STATUS_ENUM.RL)) != 0;
                if (isPel)
                {
                    return true;
                }
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController GetMELStatus Exception:" + ex.StackTrace);
            }

            return false;
        }

        public bool GetServoOnStatus(short axisId)
        {
            bool isServoOn = false;
            try
            {
                isServoOn = (ReadIntIndex("MST", axisId) & (1 << (int)ACS_MOTION_STATUS_ENUM.MTS_ENABLED)) != 0;
                if (isServoOn)
                {
                    return true;
                }
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController GetServoOnStatus Exception:" + ex.StackTrace);
            }

            return false;
        }

        public bool GetTargetPosition(short axisId, ref double targetPosition)
        {
            try
            {
                targetPosition = Ch.GetTargetPosition((ACS.SPiiPlusNET.Axis)axisId);
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController GetTargetPosition Exception:" + ex.StackTrace);
            }
            return true;
        }

        public void WriteVariable(object value, string variableName)
        {
            Ch.WriteVariable(value, variableName);
        }


        public void WriteVariableIndex(object value, string variableName, int index)
        {
            Ch.WriteVariable(value, variableName, ProgramBuffer.ACSC_NONE, index, index);

        }

        public void WriteVariableBuffer(object value, string variableName, int bufferId)
        {
            Ch.WriteVariable(value, variableName, (ProgramBuffer)bufferId);
        }
        public bool HomeMove(short axisId, PRA_HOME_ENUM homeMode, DIR_ENUM homeDir, PRA_SF_ENUM homeCure, double homeAcc, double homeVm, double homeVo, double homeShiftDistance, double stopDec, SWITCH_ENUM isEnableSoftLimit, double spelPos, double smelPos)
        {
            int mode = 0;
            int bufferId = 1;

            if (homeMode == PRA_HOME_ENUM.HOME_MODE_EL)
            {
                if (homeDir == DIR_ENUM.Positive)
                {
                    mode = 18;
                }
                if (homeDir == DIR_ENUM.Negative)
                {
                    mode = 17;
                }
            }
            else if (homeMode == PRA_HOME_ENUM.HOME_MODE_EZ)
            {
                if (homeDir == DIR_ENUM.Positive)
                {
                    mode = 2;
                }
                if (homeDir == DIR_ENUM.Negative)
                {
                    mode = 1;
                }
            }
            //WriteVariableBuffer((int)axisId, "axisId", axisId);
            //WriteVariableBuffer((int)mode, "homeMode", axisId);
            //WriteVariableBuffer((int)homeAcc, "homeAcc", axisId);
            //WriteVariableBuffer((int)homeVm, "homeVm", axisId);

            Ch.StopBuffer((ProgramBuffer)axisId + 1);
            Ch.RunBuffer((ProgramBuffer)axisId + 1, null);

            return true;
        }

        public bool InterPolationLine(int[] axisIdArray, double[] axisPosArray, PRA_OPTION_ENUM apsOption, double vm, double acc = 0.1, double dec = 0.1, PRA_SF_ENUM SF = PRA_SF_ENUM.S_CURVE)
        {
            int? ret = 0;
            double[] positionArray = new double[10];
            double[] Vm = new double[10];
            double[] Acc = new double[10];
            double[] Dec = new double[10];
            try
            {
                if (axisIdArray.Length == axisPosArray.Length)
                {
                    for (int axisNumIndex = 0; axisNumIndex < axisPosArray.Length; axisNumIndex++)
                    {
                        Vm[axisNumIndex] = vm /** axisMagnification[axisIdArray[axisNumIndex]].AXIS_MAGNIFICATION*/;
                        Acc[axisNumIndex] = /*Vm[axisNumIndex] /*/ acc;
                        Dec[axisNumIndex] = /*Vm[axisNumIndex] /*/ dec;
                        positionArray[axisNumIndex] = axisPosArray[axisNumIndex] /** axisMagnification[axisIdArray[axisNumIndex]].AXIS_MAGNIFICATION*/;
                    }
                    P2PMoveWait(axisIdArray, axisPosArray, apsOption, Vm, Acc, Dec);
                    if (ret != 0)
                    {
                        //m_Logger.Error("轴" + axisIdArray[axisNumIndex] + "InterPolationLine失败！");
                        //return false;
                    }
                }
            }
            catch (Exception ex)
            {
                //m_Logger.Error("多轴直线插补运动失败！" + ex.StackTrace);
                return false;
            }
            return true;
        }

        public bool IsMoving(int axisNum)
        {
            bool isINP = false;
            try
            {
                //0： Motor is moving or is out of range; 1: Motor is not moving and has reached the target position(see variables TARGRAD and SETTLE
                isINP = (ReadIntIndex("MST", axisNum) & (1 << (int)ACS_MOTION_STATUS_ENUM.MTS_INP)) == 0;//0  is moving or is out of range
                if (isINP)
                {
                    return true;
                }
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController IsMoving Exception:" + ex.StackTrace);
            }

            return false;
        }

        public bool JogMoveStart(short axisId, DIR_ENUM apsOption, double jogVm, double jogAcc)
        {
            if (apsOption == DIR_ENUM.Negative)
            {
                jogVm = -jogVm;
            }
            try
            {
                Ch.Jog(MotionFlags.ACSC_AMF_VELOCITY, (ACS.SPiiPlusNET.Axis)axisId, jogVm);
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController JogMoveStart Exception:" + ex.StackTrace);
            }
            return true;
        }

        public bool JogMoveStop(short axisId)
        {
            try
            {
                Ch.Halt((ACS.SPiiPlusNET.Axis)axisId);
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController JogMoveStop Exception:" + ex.StackTrace);
            }
            return true;
        }

        public bool LoadParamFile(string filePath, int boardId)
        {
            throw new NotImplementedException();
        }

        public bool MoveVel(short axisId, DIR_ENUM dir, double acc, double dec, double vm, double SF = 0.5)
        {
          return  JogMoveStart(axisId, dir, vm, acc);
        }

        public void P2PMoveWait(int[] axisIdArray, double[] axisPosArray, PRA_OPTION_ENUM apsOption, double[] vm, double[] acc, double[] dec)
        {
            int ret = 0;
            for (int i = 0; i < axisIdArray.Length; i++)
            {
                int axisId = axisIdArray[i];

                PTPMove((short)axisId, apsOption, vm[i], acc[i], dec[i], (Double)(axisPosArray[i]));
            }
        }

        public void PointTableMove(int _cardId, int axis0, int axis1, double[] x, double[] y, double vm, double acc)
        {
            throw new NotImplementedException();
        }
        public bool ClearAlarm(short axisId)
        {
            try
            {
                Ch.FaultClear((ACS.SPiiPlusNET.Axis)axisId);
                return true;

            }
            catch (Exception ex)
            {
                throw new Exception("GetALMStatus Exception! " + ex.StackTrace);
            }

        }
        public bool PTPMove(short axisId, PRA_OPTION_ENUM ApsOption, double vm, double acc, double dec, double position)
        {
            try
            {
                if (ApsOption == PRA_OPTION_ENUM.Absolute)
                {
                    Ch.SetVelocity((ACS.SPiiPlusNET.Axis)axisId, vm);
                    Ch.SetAcceleration((ACS.SPiiPlusNET.Axis)axisId, acc);
                    Ch.SetDeceleration((ACS.SPiiPlusNET.Axis)axisId, acc);
                    Ch.ToPoint(MotionFlags.ACSC_AMF_WAIT, (ACS.SPiiPlusNET.Axis)axisId, position);
                    Ch.Go((ACS.SPiiPlusNET.Axis)axisId);
                    Thread.Sleep(100);
                }
                else if (ApsOption == PRA_OPTION_ENUM.Relative)
                {
                    Ch.SetVelocity((ACS.SPiiPlusNET.Axis)axisId, vm);
                    Ch.SetAcceleration((ACS.SPiiPlusNET.Axis)axisId, acc);
                    Ch.SetDeceleration((ACS.SPiiPlusNET.Axis)axisId, acc);
                    Ch.ToPoint(MotionFlags.ACSC_AMF_RELATIVE, (ACS.SPiiPlusNET.Axis)axisId, position);
                    Ch.Go((ACS.SPiiPlusNET.Axis)axisId);
                    Thread.Sleep(100);
                }
                return true;
            }
            catch (Exception ex)
            {
                throw new Exception("ACSController PTPMove Exception:" + ex.StackTrace);
            }
        }

        public bool ServoOff(int axisId)
        {
            try
            {
                Ch.Disable((ACS.SPiiPlusNET.Axis)axisId);
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController ServoOff Exception:" + ex.StackTrace);
            }
            return true;
        }

        public bool ServoON(int axisId)
        {
            try
            {
                Ch.Enable((ACS.SPiiPlusNET.Axis)axisId);
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController ServoON Exception:" + ex.StackTrace);
            }
            return true;
        }



        public bool SetPos(int axisId, double pos)
        {
            try
            {
                Ch.SetFPosition((ACS.SPiiPlusNET.Axis)axisId, pos);
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController SetPos Exception:" + ex.StackTrace);
            }
            return true;
        }

        public bool StopEmg(short axisId)
        {
            try
            {
                Ch.Halt((ACS.SPiiPlusNET.Axis)axisId);
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController Halt Exception:" + ex.StackTrace);
            }
            return true;
        }

        public bool StopMove(short axisId, double acc)
        {
            try
            {
                Ch.Halt((ACS.SPiiPlusNET.Axis)axisId);
            }
            catch (Exception ex)
            {

                throw new Exception("ACSController StopMove Exception:" + ex.StackTrace);
            }
            return true;
        }


        public bool GetDI(int axisId, int port = 0)
        {
            bool status;
            try
            {
                int value = ReadIntIndex("IN", axisId);
                port = Convert.ToInt16(port);
                status = (value & (1 << port)) != 0;
            }
            catch (Exception ex)
            {
                throw new Exception("ACSController GetDI Exception:" + ex.StackTrace);
            }
            return status;
        }

        public bool GetDO(int axisId, int port = 0)
        {
            bool status;
            try
            {
                int value = ReadIntIndex("OUT", axisId);
                port = Convert.ToInt16(port);
                status = (value & (1 << port)) != 0;
            }
            catch (Exception ex)
            {
                throw new Exception("ACSController GetDO Exception:" + ex.StackTrace);
            }
            return status;
        }

        public bool SetDO(int axisId, int port, int value = 0)
        {
            try
            {
                int oldValue = ReadIntIndex("OUT", axisId);
                value = oldValue | (value << port);
                WriteVariableIndex(value, "OUT", axisId);
            }
            catch (Exception ex)
            {
                throw new Exception("ACSController SetDO Exception:" + ex.StackTrace);
            }
            return true;
        }

        public bool AddContinueInterpolationPos_Line(double vel, List<int> axisIds, List<double> xPos, List<double> yPos, List<double> zPos)
        {
            try
            {
                Ch.StopBuffer(ProgramBuffer.ACSC_BUFFER_6);
                Ch.WriteVariable(vel, "vel", ProgramBuffer.ACSC_BUFFER_6);
                Ch.WriteVariable(xPos.Count - 1, "Length", ProgramBuffer.ACSC_BUFFER_6);
                Ch.WriteVariable(xPos.ToArray(), "xPos", ProgramBuffer.ACSC_BUFFER_6, 0, xPos.Count - 1);
                Ch.WriteVariable(yPos.ToArray(), "yPos", ProgramBuffer.ACSC_BUFFER_6, 0, yPos.Count - 1);
                Ch.WriteVariable(zPos.ToArray(), "zPos", ProgramBuffer.ACSC_BUFFER_6, 0, zPos.Count - 1);
                Ch.RunBuffer(ProgramBuffer.ACSC_BUFFER_6, null);
            }
            catch (Exception)
            {
                return false;
            }
            return true;
        }
        public bool AddContinueInterpolationPos_Arc(double vel, List<int> axisIds, double start_X, double start_Y, double center_X, double center_Y, double angle)
        {
            try
            {
                Ch.StopBuffer(ProgramBuffer.ACSC_BUFFER_6);
                Ch.WriteVariable(vel, "vel", ProgramBuffer.ACSC_BUFFER_6);
                Ch.WriteVariable(start_X, "StartX", ProgramBuffer.ACSC_BUFFER_6);
                Ch.WriteVariable(start_Y, "StartY", ProgramBuffer.ACSC_BUFFER_6);
                Ch.WriteVariable(center_X, "CenterX", ProgramBuffer.ACSC_BUFFER_6);
                Ch.WriteVariable(center_Y, "CenterY", ProgramBuffer.ACSC_BUFFER_6);
                Ch.WriteVariable(angle, "Angle", ProgramBuffer.ACSC_BUFFER_6);
                Ch.WriteVariable(axisIds.ToArray(), "axisIds", ProgramBuffer.ACSC_BUFFER_6, 0, axisIds.Count - 1);
                Ch.RunBuffer(ProgramBuffer.ACSC_BUFFER_6, null);
            }
            catch (Exception ex)
            {
                return false;
            }
            return true;
        }

        public bool StartContinueInterpolation(List<int> axisIds, double vel)
        {
            try
            {
                //启动主轴即可
                Ch.SetVelocityImm((ACS.SPiiPlusNET.Axis)axisIds[0], vel);
                //使对应坐标系开始运行连续插补运动  
                Ch.Go((ACS.SPiiPlusNET.Axis)axisIds[0]);
            }
            catch (Exception ex)
            {
                return false;
            }

            return true;
        }
        public bool StopContinueInterpolation(List<int> axisIds)
        {
            try
            {
                Ch.SetVelocityImm((ACS.SPiiPlusNET.Axis)axisIds[0], 0);//将主轴速度设置成0
            }
            catch (Exception)
            {
                return false;
            }
            return true;
        }
        /// <summary>
        /// 等待连续插补运动完成
        /// </summary>
        /// <param name="crd"></param>
        /// <param name="fifo"></param>
        /// <returns></returns>
        public bool WaitContinueInterpolationDone(List<int> axisIds)
        {
            bool isMoving = false;
            try
            {
                foreach (var axisId in axisIds)
                {
                    do
                    {
                        Thread.Sleep(10);
                        //获取插补状态
                        isMoving = IsMoving(axisId);
                    }
                    while (isMoving == true);
                }
            }
            catch (Exception ex)
            {
                return false;
            }

            return true;
        }

        public bool StartGear(short axisId, GEAR_MODE mode)
        {
            throw new NotImplementedException();
        }

        public bool IsHoming(int axisID)
        {
            return IsMoving(axisID);
        }

        public void UpdateSlaveResource(ref ICard card, int mode = 0)
        {
            throw new NotImplementedException();
        }
    }
}
